#include "user_bsp.h"
#include "system.h"
//#include "ctrl_control.h"

//Demo_St_Act_t Demo_St_Act = Demo_St_Act_Climb;
//Demo_StToSp_Act_t Demo_StToSp_Act = Demo_StToSp_Act_Climb;
//Demo_StLdLu_Act_t Demo_StLdLu_Act = Demo_StLdLu_Act_Climb;
//Demo_t SysDemo = Demo_St;
//uint16 Demo_Act_Num = 0;
//uint16 Demo_Act_Cnt = 0;
uint16 HomeHeight_cm = 18;   // Updated when user press the start key


void SysCtrl_Update_PID()
{
//   YesNo_t Sample_Valid = Yes;
//   if( EN_PID_Calcu == Yes)
//    {
        SysPID[Rol].set = CAMERA_Fit_W / 2	;

        SysPID[Pit].set = CAMERA_Fit_H / 2;

        SysPID[Alt].set = 70;

        SysPID[Yaw].set = 0;



        PID_Update_Sample(&SysPID[Rol], Info_Car_t.Mid_xy.x, (YesNo_t)Yes);
        PID_Update_Sample(&SysPID[Pit], Info_Car_t.Mid_xy.y, (YesNo_t)Yes);
 //       PID_Update_Sample(&SysPID[Yaw], Point_t.Rotate_Offest, (YesNo_t)Yes);

        PID_Update_Error(&SysPID[Rol], Yes);
        PID_Update_Error(&SysPID[Pit], Yes);
//        PID_Update_Error(&SysPID[Yaw], Yes);

        Dof_PID_Update_Calculate(&SysPID[Rol]);
        Dof_PID_Update_Calculate(&SysPID[Pit]);
//        Dof_PID_Update_Calculate(&SysPID[Alt]);
//        Dof_PID_Update_Calculate(&SysPID[Yaw]);

//        SysCtrl_Update_PID_Change();

		EN_PID_Calcu=No;
		EN_SendMsgToFmu=Yes;
//    }
}


/*void SysCtrl_Update_PID(void)
{
   if( EN_PID_Calcu == Yes)
    {
        SysPID[Rol].PID[Pos].set = CAMERA_Fit_W / 2	;

        SysPID[Pit].PID[Pos].set = CAMERA_Fit_H / 2;

        SysPID[Alt].PID[Pos].set = 70;

        SysPID[Yaw].PID[Pos].set = 0;

        SysCtrl_Update_PID_Sample();
        SysCtrl_Update_PID_Error();


        SysCtrl_Update_PID_Calculate();

		SysCtrl_Update_PID_Change();
		EN_PID_Calcu=No;
		EN_SendMsgToFmu=Yes;
    }
}*/

void SysCtrl_Update_PID_Set(void)
{

}


/*void SysCtrl_Update_PID_Sample(void)
{
    
   	PID_Update_Sample(&SysPID[Rol].PID[Pos], Point_t.mid_x, (YesNo_t)Yes);
    
    PID_Update_Sample(&SysPID[Pit].PID[Pos], Point_t.mid_y, (YesNo_t)Yes);
    
    PID_Update_Sample(&SysPID[Yaw].PID[Pos], Point_t.yaw_Err, (YesNo_t)Yes);
    
}*/

//extern void SysCtrl_Update_PID_Change(void)
//{
//    switch(SysDemo)
//    {
//        case Demo_St:       SysCtrl_Update_PID_Change_Demo_St();        break;
//        case Demo_StToSp:   SysCtrl_Update_PID_Change_Demo_StToSp();    break;
//        case Demo_StLdLu:   SysCtrl_Update_PID_Change_Demo_StLdLU();    break;
//    }
//}


void SysCtrl_Update_Msg_Ctrl_To_FMU(void)
{

    SysCtrl_Update_Msg_Ctrl_To_FMU_Type(Auto_On);
		if(EN_SendMsgToFmu==Yes)
		{
			while(sci_sendOK & 0x80 != 0x80);
			sci_sendOK  = sci_sendOK & (~0x80);
			R_SCI5_Serial_Send(Msg_CtrlToFMU.Buf, sizeof(Msg_CtrlToFMU_t));
			EN_SendMsgToFmu=No;
		}
    
}

void SysCtrl_Update_Msg_Ctrl_To_FMU_Type(Msg_CtrlToFMU_Type_t MsgType)
{
    Msg_CtrlToFMU.Pkg.St = 0xAAAA;
    Msg_CtrlToFMU.Pkg.Sp = 0x5555;
    AutoCtrl_HeightAbove_35cm=Yes;
    switch(MsgType)
    {
			case Auto_On:
			{
					if(Yes == AutoCtrl_HeightAbove_35cm)
					{
	
							Msg_CtrlToFMU.Pkg.Rol = SysPID[Rol].Out_Int16;						
							Msg_CtrlToFMU.Pkg.Pit = SysPID[Pit].Out_Int16;											
							Msg_CtrlToFMU.Pkg.Yaw = SysPID[Yaw].Out_Int16;

						}
					else
					{
							Msg_CtrlToFMU.Pkg.Rol = 0;
							Msg_CtrlToFMU.Pkg.Pit = 0;
							Msg_CtrlToFMU.Pkg.Yaw = 0;
					}
			}
        break;
        default:
        {
            Msg_CtrlToFMU.Pkg.Rol = 0;
            Msg_CtrlToFMU.Pkg.Pit = 0;
            Msg_CtrlToFMU.Pkg.Alt = 0;
            Msg_CtrlToFMU.Pkg.Yaw = 0;
        }
        break;
    }
}
